#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <linux/fs.h>
#include <errno.h>
#include <string.h>
#include <math.h>
#include <linux/i2c-dev.h>
#include <linux/i2c.h>
#include <SDL2/SDL.h>
#include <SDL2/SDL_image.h>

#include "ICM20948.h"
#include "event.h"
#include "parameter.h"

#include "log2file.h"


#define sleepMS(x) usleep(1000 * x)
#define RAD_TO_DEG (180.0/3.14159)
/*
	标准i2c读
	写8位地址，读8位数据
*/

static int read_reg(unsigned char i2c_addr, unsigned char reg_addr,  unsigned char *pval)
{
	unsigned char  reg[2];
	unsigned char  rbuf[4];
	reg[0] = reg_addr;

	int i2cfd = open("/dev/i2c-1", O_RDWR);
	if(i2cfd < 0) {
		m_ERROR("[%s] ERROR: open(%d) failed\n", __func__,i2cfd);
		return -1;
	}
	
	ioctl(i2cfd,I2C_TENBIT,0);
	
	// SETTING i2c slave ADDR
	if(ioctl(i2cfd, I2C_SLAVE, i2c_addr) < 0)  
	{
		//m_ERROR("[%s] ERROR: ioctl(fd, I2C_SLAVE, 0x%02X) failed\n", __func__, i2c_addr);
		close(i2cfd);
		return -1;
	}	

	if(write(i2cfd, reg, 1)!= 1){
		//m_ERROR("[%s] ERROR: write reg[0x%02x] failed\n", __func__, reg[0]);
		close(i2cfd);
		return -1;
	}

	if(read(i2cfd, rbuf,1) != 1){
		//m_ERROR("[%s] ERROR: read reg[0x%02x] failed\n", __func__, reg[0]);
		close(i2cfd);
		return -1;
	}

	close(i2cfd);
	(*pval) =  rbuf[0];
	return 0;
}

/*
	标准i2c写
	写8位地址，写8位数据
*/
static int write_reg(unsigned char i2c_addr, unsigned char RegAddr,unsigned char val)
{
	unsigned char  reg[8];
	//unsigned char  rbuf[4];
	//unsigned char  i;
	//地址高位在先
	reg[0] = RegAddr;
	reg[1] = val;

	int i2cfd = open("/dev/i2c-1", O_RDWR);
	if (i2cfd < 0) {
		m_ERROR("[%s] ERROR: open(%d) failed\n", __func__, i2cfd);
		return -1;
	}

	ioctl(i2cfd,I2C_TENBIT,0);
	// SETTING i2c slave ADDR
	if (ioctl(i2cfd, I2C_SLAVE, i2c_addr) < 0)  
	{
		//m_ERROR("[%s] ERROR: ioctl(fd, I2C_SLAVE, 0x%02X) failed\n", __func__, i2c_addr);
		close(i2cfd);
		return -1;
	}	
	
	//set reg addr to read
	if(write(i2cfd, reg, 1+1)!= 1+1){
		//m_ERROR("[%s] ERROR: write reg[0x%02x] failed\n", __func__, reg[0]);
		close(i2cfd);
		return -1;
	}

	close(i2cfd);
	return 0;
}

static uint8_t IIC_Read_Configure(uint8_t addr,uint8_t Reg)
{
	uint8_t Data = 0;
	read_reg(addr,Reg,&Data);
	return Data;
}

static void IIC_Write_Configure(uint8_t addr,uint8_t Reg, uint8_t Data)
{	
	write_reg(addr,Reg,Data);
}

static void writeByte(uint8_t regAddr, uint8_t value)
{
	IIC_Write_Configure(0x68,regAddr,value);
}

static uint8_t readByte(uint8_t regAddr)
{
	return IIC_Read_Configure(0x68,regAddr);
}

static int readNBytes(uint8_t regAddr, uint8_t length, uint8_t* data)
{

	int i2cfd = open("/dev/i2c-1", O_RDWR);
	if(i2cfd < 0) {
		//m_ERROR("[%s] ERROR: open(%d) failed\n", __func__,i2cfd);
		return -1;
	}
	
	ioctl(i2cfd,I2C_TENBIT,0);
	
	// SETTING i2c slave ADDR
	if(ioctl(i2cfd, I2C_SLAVE, 0x68) < 0)  
	{
		//m_ERROR("[%s] ERROR: ioctl(fd, I2C_SLAVE, 0x%02X) failed\n", __func__, i2c_addr);
		close(i2cfd);
		return -1;
	}	

	if(write(i2cfd, &regAddr, 1)!= 1){
		//m_ERROR("[%s] ERROR: write reg[0x%02x] failed\n", __func__, reg[0]);
		close(i2cfd);
		return -1;
	}

	if(read(i2cfd, data,length) != length){
		m_ERROR("[%s] ERROR: read reg[0x] failed\n", __func__ );
		close(i2cfd);
		return -1;
	}

	close(i2cfd);
	return 0;
}

static void magI2CRead(uint8_t regAddr, uint8_t length, uint8_t* data)
{
	writeByte(0x7F,0x30);  // 设置BANK3
    writeByte(0x03, 0x0c | 0x80);
    writeByte(0x04,  regAddr);
    writeByte(0x05, 0x80 | length);

    writeByte(0x7F,0x00);  // 设置BANK0
    readNBytes(0x3B, length, data);
}

static void magI2CWrite(uint8_t regAddr, uint8_t value)
{
	uint8_t u8Temp;
	writeByte(0x7F,0x30);  // 设置BANK3
	writeByte(0x03, 0x0C | 0x00);
	writeByte(0x04,  regAddr);
	writeByte(0x06,   value);
	writeByte(0x05, 0x80 | 1);

	sleepMS(100);
	magI2CRead(regAddr, 1, &u8Temp);
}

static void setBank(const ICM_BANK bank)
{
    writeByte(REG_ADD_REG_BANK_SEL, bank);
}

static void reset(void)
{
    uint8_t sensorsFlag = REG_VAL_SENSORS_ON;

	magI2CWrite(REG_ADD_MAG_CNTL2, 0x00);
	sleepMS(100);

	setBank(BANK_0);
	/* Reset all settings on master device  */
	writeByte(REG_ADD_PWR_MGMT_1, REG_VAL_ALL_RGE_RESET);

	sleepMS(10);
	/* Enable optimal on-board timer and configure temperature sensor */
	writeByte(REG_ADD_PWR_MGMT_1, REG_VAL_RUN_MODE | (0) << 3);

	/* Enable both sensors */
	if (0)
	{
		sensorsFlag |= REG_VAL_DISABLE_GYRO;
	}
	if (0)
	{
		sensorsFlag |= REG_VAL_DISABLE_ACC;
	}
	writeByte(REG_ADD_PWR_MGMT_2, sensorsFlag);
	sleepMS(10);

	/* Reset all settings on magnetometer.
	 * NOTE: this will log error as the value is immediately changed back to 0 by the sensor itself.  */
	magI2CWrite(REG_ADD_MAG_CNTL3, REG_VAL_MAG_RESET);
	sleepMS(100);
}

static void configureMasterI2C(void) 
{
	uint8_t temp;
	setBank(BANK_0);
	/* Read the current user control and update it with new configuration to enable I2C master */
	temp = readByte(REG_ADD_USER_CTRL);
	writeByte(REG_ADD_USER_CTRL, temp | REG_VAL_BIT_I2C_MST_EN);

	/* Set I2C master clock to recommended value. */
	setBank(BANK_3);
	writeByte(REG_ADD_I2C_MST_CTRL, REG_VAL_I2C_MST_CTRL_CLK_400KHZ);

	sleepMS(10);
}

static void configureTemp(void) 
{
	setBank(BANK_2);
	/* configure temperature sensor */
	writeByte(REG_ADD_TEMP_CONFIG, TEMP_DLPF_BANDWIDTH_9HZ);
}

static void configureGyro(void)
{
	setBank(BANK_2);
	writeByte(REG_ADD_GYRO_SMPLRT_DIV, 4 & 0xFF);
	writeByte(REG_ADD_GYRO_CONFIG_1, GYRO_DLPF_BANDWIDTH_6HZ | (0 << 1));
}

static void configureAcc(void)
{
	ACC_DLPF_BANDWIDTH bandwidth = ACC_DLPF_BANDWIDTH_6HZ;
	uint16_t sampleRateDivisor = 4;

	setBank(BANK_2);
	writeByte(REG_ADD_ACCEL_SMPLRT_DIV_1, (sampleRateDivisor >> 8) & 0x0F);
	writeByte(REG_ADD_ACCEL_SMPLRT_DIV_2, (sampleRateDivisor     ) & 0xFF);
	writeByte(REG_ADD_ACCEL_CONFIG, bandwidth | (ACC_RANGE_2G<< 1));

}

static int checkMag(void) 
{
    uint8_t u8Ret[2];
    magI2CRead(REG_ADD_MAG_WIA1, 2, u8Ret);

    return ((u8Ret[0] == REG_VAL_MAG_WIA1) && (u8Ret[1] == REG_VAL_MAG_WIA2));
}

static int configureMag(void)
{
	uint8_t u8Data[8];
	int counter = 0;
	int flag = checkMag();

	while (!flag && (++counter <= 10))
	{
		sleepMS(100);
		flag = checkMag();
	}

	if (flag)
	{
		sleepMS(1000);
		magI2CWrite(REG_ADD_MAG_CNTL2, REG_VAL_MAG_MODE_100HZ);

		sleepMS(10);
		magI2CRead(REG_ADD_MAG_DATA, 8, u8Data);
	}

	return flag;
}

/****************************************************************
*
*
****************************************************************/
uint8_t ICM_getID(void)
{
     return readByte(REG_ADD_WIA);
}

static int16_t mx_offset = 0,my_offset = 0,mz_offset = 0;

static int16_t mx_min = 32760,mx_max = -32760;
static int16_t my_min = 32760,my_max = -32760;
static int16_t mz_min = 32760,mz_max = -32760;

static int16_t ax_filter = 0,ay_filter = 0, az_filter = 0;
static int16_t mx_filter = 0,my_filter = 0, mz_filter = 0;


void ICM_20948_Init(void)
{
    uint8_t deviceID = 0;
    int tmp;
    setBank(BANK_0);
	deviceID = readByte(REG_ADD_WIA);
    reset();
    deviceID = readByte(REG_ADD_PWR_MGMT_2);
    
    setBank(BANK_3);
    /* Reset I2C master clock. */
    writeByte(REG_ADD_I2C_MST_CTRL, 0);
    configureMasterI2C();
    
    configureTemp();
    configureGyro();
    configureAcc();
    deviceID = configureMag();
    if(0 == hal_get_para(PARA_COMPASS_MX, &tmp))
        mx_offset = tmp;
    if(0 == hal_get_para(PARA_COMPASS_MY, &tmp))
        my_offset = tmp;
    if(0 == hal_get_para(PARA_COMPASS_MZ, &tmp))
        mz_offset = tmp;    
}

/* 读取加速度计的值 */
static void readRawAcc(void) 
{
    uint8_t u8Buf[6];
    int16_t acc[3];
	setBank(BANK_0);
    readNBytes(REG_ADD_ACCEL_XOUT_H, 6, u8Buf);
    acc[0] = ((u8Buf[0]) << 8) | (u8Buf[1]);
    acc[1] = ((u8Buf[2]) << 8) | (u8Buf[3]);
    acc[2] = ((u8Buf[4]) << 8) | (u8Buf[5]);

    ax_filter = acc[0];
	ay_filter = acc[1];
	az_filter = acc[2];
}

/* 读取罗盘的值 */
static int readRawMag(void) 
{
    uint8_t u8Buf[8];
    int16_t mag[3];
	setBank(BANK_0);
    readNBytes(REG_ADD_EXT_SENS_DATA_00, 8, u8Buf);
    mag[0] =  ((u8Buf[1]) << 8) | (u8Buf[0]);
    mag[1] =-(((u8Buf[3]) << 8) | (u8Buf[2]));
    mag[2] =-(((u8Buf[5]) << 8) | (u8Buf[4]));

    mx_filter = mag[0];
	my_filter = mag[1];
	mz_filter = mag[2];
    
    /* Check if there was an overflow. */
    return (u8Buf[7] & 0x08);
}


// 参数 等于 1 进入低功耗，0,退出低功耗
// 返回值 等于 1 设置成功
int8_t Enter_sleep_mode(int8_t flag)
{
	int8_t temp;
	setBank(BANK_0);
	temp = readByte(REG_ADD_PWR_MGMT_1);
	if(flag)
	{
		temp = temp|0x40;  //bit6 置 1
	}
	else
	{
		temp = temp & 0xBF; // bit6 清0
	}

	writeByte(REG_ADD_PWR_MGMT_1,temp);

	temp = readByte(REG_ADD_PWR_MGMT_1);

	return (((temp>>6) & 0x01) == flag); // 检测有没有设置成功

}

int  Hal_ICM20948_calibration(int *pval)
{
    static uint32_t i = 0;

    mx_min = 32760;mx_max = -32760;
    my_min = 32760;my_max = -32760;
    mz_min = 32760;mz_max = -32760;
    mx_filter = 0;
    my_filter = 0;
    mz_filter = 0;

    do {
		if(0 == *pval) {
            ICM_20948_Init();
			return 0;
		}
		
		readRawMag();

        if(mx_min > mx_filter) 
                mx_min = mx_filter;
        if(mx_max < mx_filter)
                mx_max = mx_filter;
        if(my_min > my_filter) 
                my_min = my_filter;
        if(my_max < my_filter) 
                my_max = my_filter;
        if(mz_min > mz_filter) 
                mz_min = mz_filter;
        if(mz_max < mz_filter) 
                mz_max = mz_filter;
        usleep(300);
	    i++;
	}while(i<1000*8);

    mx_offset = (mx_min + mx_max)/2;

    my_offset = (my_min + my_max)/2;

    mz_offset = (mz_min + mz_max)/2;

    i = 0;

    hal_save_para(PARA_COMPASS_MX, mx_offset);
	hal_save_para(PARA_COMPASS_MY, my_offset);
	hal_save_para(PARA_COMPASS_MZ, mz_offset);
	
    return 0;
}


/*
 * 算法坐标系  东 南 天，
 * 电子罗盘需要根据贴片位置和此坐标系对应。
 * @benableMag  罗盘开关
 * @benableACC  加速度计开关
 * @pfw_angle   罗盘角度
 * @phori_angle 横滚角
 * @pvert_angle 俯仰角
 */
void Hal_ICM20948(unsigned int benableMag,
					unsigned int benableACC,
					int *pfw_angle,
					int *phori_angle,
					int *pvert_angle)
{
	float hori_angle = 0, vert_angle = 0;
	float ax_f = 0.0,ay_f = 0.0,az_f = 0.0;	
	float mx_f = 0.0,my_f = 0.0,mz_f = 0.0;
	float temp = 0.0;
	float tempNormAcce,tempNormYZ;
	float fSinRoll = 0,fCosRoll = 0,fSinPitch = 0,fCosPitch = 0;
    float xh = 0.0,yh = 0.0;

    if((!benableMag) && (!benableACC))
		return;

    readRawAcc();
    readRawMag();
    
	ax_f = (((float)ay_filter));//加速度计与  东 南 天 坐标系转换 
	ay_f = (((float)az_filter));
	az_f = (((float)-ax_filter));
	
     if(ax_f==0)
         ax_f=1;
     if(ay_f==0)
         ay_f=1;
     if(az_f==0)
         az_f=1;
			 
	if(	benableMag == 1){	 
    	temp = ax_f*ax_f + ay_f*ay_f +  az_f*az_f;
    	tempNormAcce = sqrt(temp);
    	temp = ay_f*ay_f+ az_f*az_f;
    	tempNormYZ = sqrt(temp);
    	fSinPitch = ax_f/tempNormAcce;
    	temp = 1.0 - (fSinPitch * fSinPitch);
    	fCosPitch = sqrt(temp);
    	fSinRoll = ay_f/tempNormYZ;
    	fCosRoll = az_f/tempNormYZ;
    	
    	mx_f = (((float)-my_filter))+my_offset;//磁力计与 东 南 天 坐标系转换，my_offset是校准值
    	my_f = (((float)-mz_filter))+mz_offset;
    	mz_f = (((float)-mx_filter))+mx_offset;
    	
    	yh = mz_f * fSinRoll - my_f * fCosRoll;
    	xh = mx_f * fCosPitch + my_f * fSinPitch * fSinRoll + mz_f * fCosRoll * fSinPitch;
    	
    	temp=atan2(yh , xh);
    	temp *= RAD_TO_DEG;
    	temp += 90;
    	if(temp < 0)
    		temp += 360;
    	(*pfw_angle) = (int)temp;
	}
	if(benableACC == 1){
		hori_angle=(float)(atan2(ax_f,az_f) * RAD_TO_DEG);
		vert_angle=(float)(atan2(ay_f,az_f) * RAD_TO_DEG);
		
		if(hori_angle>=90)
            hori_angle=90;
        else if(hori_angle<=-90)
            hori_angle=-90;

        if(vert_angle>90)
            vert_angle=90;
        else if(vert_angle<-90)
            vert_angle=-90;
		
		(*phori_angle) = (int)-hori_angle;
		(*pvert_angle) = (int)-vert_angle;
	}
}
